function particle= add_feature(particle, z,R)
% add new features 

lenz= size(z,2);
xf= zeros(3,lenz);
Pf= zeros(3,3,lenz);
xv= particle.xv;

for i=1:lenz
    xi = z(1,i);
    yi = z(2,i);
    zi = z(3,i);
    
    xf(:,i)= [xv(1) + xi;  
              xv(2) + yi;
              xv(3) + zi];
  
    yy = (xv(4)); % roll (x)
    pp = (xv(5)); % pitch (y)
    rr = (xv(6)); % yaw (z)

    cr = cos(rr);    sr = sin(rr);
    cp = cos(pp);    sp = sin(pp);
    cy = cos(yy);    sy = sin(yy);

    Gz = [cy*cp    cy*sp*sr-sy*cr     cy*sp*cr+sy*sr;
          sy*cp    sy*sp*sr+cy*cr     sy*sp*cr-cy*sr;
          -sp      cp*sr              cp*cr         ];
    Pf(:,:,i)= Gz*R*Gz';
end

lenx= size(particle.xf,2);
ii= (lenx+1):(lenx+lenz);
particle.xf(:,ii)= xf;
particle.Pf(:,:,ii)= Pf;

